%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%
  %----- [ Initialization ] ------------------------------------------------%
clear all;
%clear classes;
close all;

if isunix
	fprintf(1,'Excellent, intelligent user at console, you''re using UNIX!');
else
	fprintf(1,'WARNING -- THIS IS NOT INTENDED FOR USE IN WINBLOWS.  PLEASE GET A REAL OPERATING SYSTEM BEFORE USE!');
end

  %----- [ Path Setup ] ----------------------------------------------------%
path(path,'scripts');
path(path,'classes');
settings.datadir = 'data/';

  %----- [ Weather ] -------------------------------------------------------%

if exist('my_weather') == 0
  my_weather = Weather(settings);
end

  %----- [ Aircraft ] ------------------------------------------------------%
clear my_ua;

if exist('my_ua') == 0
  my_ua = Aircraft();
  my_ua.flight_computer = FlightComputer('p');
  %my_ua.flight_computer = FlightComputer('c');
end

my_ua.velocity = 2;                        % m/s
my_ua.position = [5,5,500];                % m  sensing start
my_ua.euler    = [0,0,0];                  % rad
my_ua.input    = [2,0];                    % [m/s rad/s]

  %----- [ Simulation Parameters ]------------------------------------------%
target.x = 10;
target.y = 10;
target.radius = 5;

%cost_map = GenerateCostMap(my_weather,my_ua,target,my_weather.t0,my_weather.z0);
%PlotCostFunction(my_weather,cost_map,target,2)

settings.sim.ts = 1; %s
settings.sim.t0 = my_weather.t0; %s
settings.sim.tf = my_weather.tf+1200; %s
settings.sim.tnum = floor((settings.sim.tf-settings.sim.t0)/settings.sim.ts); %s
settings.sim.runplot = true;
settings.sim.figure = 4;

  %----- [ Simulation ]-----------------------------------------------------%
%plot(my_weather,my_weather.t0,my_weather.z0,'rain wind',1);
%axis([2.75*10^4 7*10^4 3.75*10^4 8*10^4])

if settings.sim.runplot == false
  h = waitbar(0,'Performing Simulation');
end

for time=settings.sim.t0:settings.sim.ts:settings.sim.tf
  if settings.sim.runplot == true
    PlotSimulation(my_weather,my_ua,target,time,settings.sim.figure);
  else
    waitbar((time-settings.sim.t0)/(settings.sim.tf-settings.sim.t0),h);
  end
  my_ua = move(my_ua,my_weather,target,time,settings.sim.ts);
  pos = my_ua.position;
  if norm([pos(1) pos(2)]-[target.x target.y]) < target.radius
    break;
  end
  if mod(time,60) == 0 && time ~= my_weather.t0
    target=FindTarget(settings,my_weather,target,time,pos(3));
  end
end

if settings.sim.runplot == false
  close(h);
  clear h;
end

clear time;
my_logfile = my_ua.log;

return

if settings.sim.runplot == false
  %frame = 1;
  for t=1:10:settings.sim.tnum
    time = t * settings.sim.ts + settings.sim.t0;
    my_ua.velocity=my_logfile.velocity(t,:);
    my_ua.position=my_logfile.position(t,:);
    my_ua.euler=my_logfile.euler(t,:);
    PlotSimulation(my_weather,my_ua,target,time,settings.sim.figure);
  if mod(time,60) == 0 && time ~= my_weather.t0 
    target=FindTarget(settings,my_weather,target,time,pos(3));
  end
    %M(frame) = getframe;
    %frame=+1;
  end
end

target.x = 2.05*10^4;
target.y = 2.6*10^4;
